#使用open3d 可视化视角
import open3d as o3d
import numpy as np
import cv2
import os
import sys
import json
import sys

sys.path.append(os.getcwd())
import open3d as o3d
import numpy as np

from mask.create_mask import get_camera_intrinsic

folder = r"E:\pose\datasets\obj_ac\obj_000002\train_pbr\000001"
camera_intrinsics, K = get_camera_intrinsic(folder)
scene = o3d.io.read_triangle_mesh(r"E:\pose\datasets\obj_ac\obj_000002\models\obj_000002.ply")

WIDTH = 640
HEIGHT = 480

def get_mul_estimated_pose(folder,n):
    import random
    length = len(os.listdir(folder))
    poses = []
    for i in random.sample(range(length),n):
        pose = np.loadtxt(f"{folder}/pose/{i}.txt").reshape(3, 4)
        pose = np.vstack((pose,[0,0,0,1]))
        poses.append(pose)
    return np.array(poses)

def vis_camera_view(n,folder,scene):
    camera_intrinsics, K = get_camera_intrinsic(folder)
    extrinsics = get_mul_estimated_pose(folder,n)
    scene = scene.scale(1, center=scene.get_center())
    vizualizer = o3d.visualization.Visualizer()
    vizualizer.create_window(width=WIDTH, height=HEIGHT)
    vizualizer.add_geometry(scene)

    for i in range(n):
        extrinsic = extrinsics[i]
        cameraLines = o3d.geometry.LineSet.create_camera_visualization(view_width_px=camera_intrinsics['width'],
                                                                       view_height_px=camera_intrinsics['height'],
                                                                       intrinsic=K[:3, :3], extrinsic=extrinsic, scale=0.05)
        vizualizer.add_geometry(cameraLines)

    vizualizer.run()


if __name__ == '__main__':
    vis_camera_view(10,folder,scene)

